/**
  ******************************************************************************
  * Copyright (C) 2020 - ~, SCUT-RobotLab Development Team
  * @file    Interpolation.h
  * @author  Mentos Seetoo
  * @brief   Trajectory generator for given points.The following splines are
  *          supported:
  *          - Cubic bezier(First/Second derivative)
  *         
  * @warning 
  *     - At least C++11 is required.			
  *     - This file refers to "Interpolation.h" in Cheetah COntrol code.											 
  ******************************************************************************
  */
#ifndef _INTERPOLATION_H
#define _INTERPOLATION_H
/* Includes ------------------------------------------------------------------*/
#include <assert.h>
#include <type_traits>
/* Types define --------------------------------------------------------------*/
namespace Interpolation
{
/*!
 * Cubic bezier interpolation between y0 and yf.  x is between 0 and 1
 */
template <typename y_t, typename x_t>
y_t cubicBezier(y_t y0, y_t yf, x_t x)
{
    static_assert(std::is_floating_point<x_t>::value,
                  "must use floating point value");
    assert(x >= 0 && x <= 1);
    y_t yDiff = yf - y0;
    x_t bezier = x * x * x + x_t(3) * (x * x * (x_t(1) - x));
    return y0 + bezier * yDiff;
}

/*!
 * Cubic bezier interpolation derivative between y0 and yf.  x is between 0 and
 * 1
 */
template <typename y_t, typename x_t>
y_t cubicBezierFirstDerivative(y_t y0, y_t yf, x_t x)
{
    static_assert(std::is_floating_point<x_t>::value,
                  "must use floating point value");
    assert(x >= 0 && x <= 1);
    y_t yDiff = yf - y0;
    x_t bezier = x_t(6) * x * (x_t(1) - x);
    return bezier * yDiff;
}

/*!
 * Cubic bezier interpolation derivative between y0 and yf.  x is between 0 and
 * 1
 */
template <typename y_t, typename x_t>
y_t cubicBezierSecondDerivative(y_t y0, y_t yf, x_t x)
{
    static_assert(std::is_floating_point<x_t>::value,
                  "must use floating point value");
    assert(x >= 0 && x <= 1);
    y_t yDiff = yf - y0;
    x_t bezier = x_t(6) - x_t(12) * x;
    return bezier * yDiff;
}
} // namespace Interpolation

#endif
/************************ COPYRIGHT(C) SCUT-ROBOTLAB **************************/